/**
 * @file ebex_steady_state.c
 *
 * @date Oct 25, 2011
 * @author seth
 *
 * @brief This file is part of EBEX State Estimator, created for the EBEX project
 *
 * This software is copyright (C) 2011 Columbia University
 *
 * EBEX State Estimator is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * EBEX State Estimator is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with EBEX State Estimator; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

#include <assert.h>
#include <memory_util.h>
#include <log_util.h>

#include <ebex_genetic.h>
#include <ebex_genetic_chromosome.h>
#include <ebex_de.h>
#include <gyros.h>
#include <ebex_quaternion.h>
#include <ebex_matrix.h>
#include <ebex_filter_cfg.h>
#include <ebex_model.h>
#include <ebex_bootstrap.h>
#include <ebex_genetic_stats.h>
#include <ebex_genetic_optimize.h>
#include <ebex_sort.h>
#include <ebex_gyro_sc.h>

#include <dirfile_data.h>

static bool ebex_steady_state_organism_seed(population_t *m_pop, organism_t *m_entity)
{

	ebex_model_t *model = (ebex_model_t*)m_pop->data;

	m_entity->fitness = GENETIC_MIN_FITNESS;
	m_entity->previous_weight = 1.0;

	for (size_t chromosome = 0; chromosome < num_gyro_params; chromosome++)
	{
		for (size_t j = 0; j < model->num_gyroboxes; j++)
		{
			for (size_t k = 0; k < 3; k++)
			{
				size_t offset = j * 3 + k;
				m_entity->chromosome[chromosome].dbl_allele[offset] = model->gyrobox[j].gyro[k].param.value.array[chromosome];
			}
		}

		/// This guarantees that the most fit organism remains in the population
		if (m_pop->size > 1) ebex_mutate_double_allpoint(m_pop, m_entity, chromosome);
	}

	for (size_t i = 0; i < model->num_gyroboxes; i++)
	{
		size_t gyrobox_chromosome = i + num_gyro_params;
		ebex_quat_copy(&m_entity->chromosome[gyrobox_chromosome].q_allele[i], &model->gyrobox[i].alignment);
		if (m_pop->size > 1) ebex_mutate_quaternion_allpoint(m_pop, m_entity, gyrobox_chromosome);
	}

	for (size_t i = 0; i < model->num_sc; i++)
	{
		size_t sc_chromosome = i + num_gyro_params + model->num_gyroboxes;
		ebex_quat_copy(&m_entity->chromosome[sc_chromosome].q_allele[i], &model->sc[i].alignment);
		if (m_pop->size > 1) ebex_mutate_quaternion_allpoint(m_pop, m_entity, sc_chromosome);
	}

	return true;
}

static bool ebex_steady_state_organism_evaluate(population_t *m_pop, organism_t *m_entity)
{
	ebex_model_t *model = (ebex_model_t*)m_pop->data;

	double mse = 0.0;
	double probability;
	size_t samples;

	ebex_quat_t start_attitude;
	ebex_quat_t attitude;
	ebex_quat_t end_attitude;

	size_t box;
	size_t i;
	double total_time = 0.0;
	gyrobox_t gyrobox = {0};
	double segment_sc_variance;

	for (box = 0; box < model->num_gyroboxes; box++)
	{
		for (size_t j = 0; j < 3; j++)
		{
			memcpy(&gyrobox.gyro[j].param, &model->gyrobox[box].gyro[j].param, sizeof(gyro_param_t));
			for (i = 0; i < num_gyro_params; i++)
			{
				size_t offset = box * 3 + j;
				gyrobox.gyro[j].param.value.array[i] = m_entity->chromosome[i].dbl_allele[offset];
			}
			gyrobox.gyro[j].param.sample.rate_noise.type = random_dist_none;

		}
		ebex_quat_copy(&gyrobox.alignment, m_entity->chromosome[box + num_gyro_params].q_allele);

#		pragma omp parallel for \
			shared(m_pop, model, mse, total_time) private(attitude, probability, samples, i) schedule(static)
		for (i = 0; i < model->data->num_segments; i++)
		{
			/// Rotate the beginning star camera reading into the gyro frame
			ebex_quat_mul(&start_attitude, &model->data->sc_segment[0].attitude, m_entity->chromosome[num_gyro_params + model->num_gyroboxes + model->data->sc_segment[0].sc_index].q_allele);
			/// Rotate the end star camera reading into the gyro frame
			ebex_quat_mul(&end_attitude, &model->data->sc_segment[1].attitude, m_entity->chromosome[num_gyro_params + model->num_gyroboxes + model->data->sc_segment[1].sc_index].q_allele);

			segment_sc_variance = pow(model->data->sc_segment[0].uncertainty.w, 2.0) + pow(model->data->sc_segment[1].uncertainty.w, 2.0);

			ebex_quat_copy(&attitude, &start_attitude);

			samples = ebex_gyro_sc_quat_propagate_forward(model, model->data->gyro_segment[box], &attitude, &gyrobox, NULL, 0, NULL, NULL, 0);

			probability = ebex_gyro_sc_get_probability(&end_attitude, &attitude, &gyrobox, samples,
					model->parameters->gyro_rate, segment_sc_variance);

			mse += (probability * probability * (model->data->sc_segment[1].time - model->data->sc_segment[0].time));
			total_time += (model->data->sc_segment[1].time - model->data->sc_segment[0].time);
		}

	}

	if (ISDBL_TINY(m_entity->fitness - GENETIC_MIN_FITNESS))
	{
		m_entity->previous_weight = 1.0 / (double)m_pop->size;
	}

	m_entity->fitness = model->parameters->steady_state.learning_ratio * sqrt(mse / total_time) +
			(1.0 - model->parameters->steady_state.learning_ratio) * m_entity->previous_weight;


	return true;
}

static void ebex_steady_state_assign_weights(double **forward_weight, double **reverse_weight, double *total_weights, size_t m_popsize, size_t m_num_obs)
{
	double weight_range = 0.05;
	double step_delta = (1.0 - 2.0 * weight_range)/(double)m_num_obs;
	double weight_scale = 2.0 * log((1.0 - weight_range)/weight_range);
	double weight[m_num_obs];

	/**
	 * There is a pathological case where the weights are all 0 (or close).  In this case, we let everyone be weighted equally
	 */
	if (ISDBL_TINY(total_weights[0]))
	{
		for (size_t elem = 0; elem < m_popsize; elem++)
		{
			forward_weight[0][elem] = 1.0;
		}
		total_weights[0] = (double)m_popsize;
	}
	if (ISDBL_TINY(total_weights[1]))
	{
		for (size_t elem = 0; elem < m_popsize; elem++)
		{
			reverse_weight[0][elem] = 1.0;
		}
		total_weights[1] = (double)m_popsize;
	}

	for (size_t obs = 0; obs < m_num_obs; obs++)
	{
		double x = (double)obs * step_delta + weight_range;
		x /= (1.0 - x);
		weight[obs] = log(x) / weight_scale + 0.5;
	}
	/// Normalize weights for forward and reverse propagation
#		pragma omp parallel for \
		shared(forward_weight, reverse_weight) schedule(static)
	for (size_t elem = 0; elem < m_popsize; elem++)
	{
		forward_weight[0][elem] /= total_weights[0];
		reverse_weight[0][elem] /= total_weights[1];
		for (size_t obs = 1; obs < m_num_obs; obs++)
		{
			forward_weight[obs][elem] = forward_weight[0][elem] * weight[m_num_obs - obs];
			reverse_weight[obs][elem] = reverse_weight[0][elem] * weight[obs];
		}
		reverse_weight[0][elem] = 0.0;
	}
}

static void ebex_steady_state_filter_attitude(ebex_model_t *m_model, ebex_quat_t **m_forward_attitude_vec, ebex_quat_t **m_reverse_attitude_vec, size_t m_numobs)
{
	double scale_factor[] = {0.0, 0.0, 0.0, 0.0, 0.0};
	double mask[] = {0.0625, 0.25, 0.375, 0.25, 0.0625};
	size_t samples = 5;
	size_t midpoint;
	size_t step_size = 2 * m_model->parameters->steady_state.state_population_size;

	if (!(samples & 1)) samples++;
	midpoint = (samples - 1) / 2;

	for (size_t i = 0; i < midpoint; i++)
	{
		for (size_t i2 = 0; i2 <= i; i2++)
			scale_factor[i] -= mask[i2];
	}
	for (size_t i = midpoint; i < samples - 1; i++)
	{
		for (size_t i2 = i + 1; i2 <= samples - 1; i2++)
			scale_factor[i] += mask[i2];
	}

	for (size_t i = 0; i < m_model->parameters->steady_state.state_population_size; i++)
	{
		ebex_quat_smooth(&m_forward_attitude_vec[0][i], step_size, &m_forward_attitude_vec[0][i], step_size, scale_factor, m_numobs, 2 * midpoint, 100, 0.5);
		ebex_quat_smooth(&m_reverse_attitude_vec[0][i], step_size, &m_reverse_attitude_vec[0][i], step_size, scale_factor, m_numobs, 2 * midpoint, 100, 0.5);
	}

}

static void ebex_steady_state_output(population_t *m_pop)
{
	ebex_model_t *model = m_pop->data;
	organism_t *best_entity = m_pop->sorted_organisms[0];

	size_t obs_size;
	ebex_3vec_t **forward_noise_vec;
	ebex_3vec_t **reverse_noise_vec;

	ebex_quat_t **forward_attitude_vec;
	ebex_quat_t **reverse_attitude_vec;
	double **forward_weight;
	double **reverse_weight;

	size_t box;
	size_t i;
	size_t restricted_num_obs;
	gyrobox_t gyrobox = {0};
	sc_param_t sc_param;

	ebex_gyro_reading_t *gyrodata;

	ebex_quat_t attitude;
	ebex_quat_t start_attitude;
	ebex_quat_t end_attitude;
	ebex_quat_t err_quat;

	ebex_quat_t sc_inv;
	ebex_mat3x3_t cov;

	double segment_sc_variance;

	for (box = 0; box < model->num_gyroboxes; box++)
	{
		double total_weights[2] = {0.0, 0.0};
		double carry_weights[2] = {0.0, 0.0};

		double kahan_temp;
		double kahan_diff;
		size_t num_obs = model->data->gyro_segment[box][0].num_obs;
		size_t size_pop = model->parameters->steady_state.state_population_size;

		gyrodata = model->data->gyro_segment[box];

		/// Rotate the beginning star camera reading into the gyro frame
		ebex_quat_mul(&start_attitude,
		              &model->data->sc_segment[0].attitude,
		              best_entity->chromosome[num_gyro_params + model->num_gyroboxes + model->data->sc_segment[0].sc_index].q_allele);
		/// Rotate the end star camera reading into the gyro frame
		ebex_quat_mul(&end_attitude,
		              &model->data->sc_segment[1].attitude,
		              best_entity->chromosome[num_gyro_params + model->num_gyroboxes + model->data->sc_segment[1].sc_index].q_allele);
		ebex_quat_inv(&sc_inv, best_entity->chromosome[num_gyro_params + model->num_gyroboxes + model->data->sc_segment[0].sc_index].q_allele);

		/// Get the relative SC errors
		segment_sc_variance = pow(model->data->sc_segment[0].uncertainty.w, 2.0) + pow(model->data->sc_segment[1].uncertainty.w, 2.0);

		ebex_quat_copy(&sc_param.alignment, best_entity->chromosome[num_gyro_params + model->num_gyroboxes].q_allele);
		for (size_t j = 0; j < 3; j++)
		{
			memcpy(&gyrobox.gyro[j].param, &model->gyrobox[box].gyro[j].param, sizeof(gyro_param_t));
			if (size_pop == 1) gyrobox.gyro[j].param.sample.rate_noise.type = random_dist_none;

			for (i = 0; i < num_gyro_params; i++)
			{
				size_t offset = box * 3 + j;
				gyrobox.gyro[j].param.value.array[i] = best_entity->chromosome[i].dbl_allele[offset];
			}
		}
		ebex_quat_copy(&gyrobox.alignment, best_entity->chromosome[box + num_gyro_params].q_allele);

		obs_size = num_obs * size_pop;

		forward_noise_vec = e_malloc(size_pop * sizeof(ebex_3vec_t*));
		reverse_noise_vec = e_malloc(size_pop * sizeof(ebex_3vec_t*));
		forward_noise_vec[0] = e_malloc(2 * obs_size * sizeof(ebex_3vec_t));
		reverse_noise_vec[0] = forward_noise_vec[0] + obs_size;
		for (i = 1; i < size_pop; i++)
		{
			forward_noise_vec[i] = forward_noise_vec[i - 1] + num_obs;
			reverse_noise_vec[i] = reverse_noise_vec[i - 1] + num_obs;
		}

		forward_attitude_vec = e_malloc(num_obs * sizeof(ebex_quat_t*));
		reverse_attitude_vec = e_malloc(num_obs * sizeof(ebex_quat_t*));
		forward_weight = e_malloc(num_obs * sizeof(double*));
		reverse_weight = e_malloc(num_obs * sizeof(double*));

		forward_attitude_vec[0] = e_malloc(2 * obs_size * sizeof(ebex_quat_t));
		reverse_attitude_vec[0] = forward_attitude_vec[0] + size_pop;
		forward_weight[0] = e_calloc(2 * obs_size, sizeof(double));
		reverse_weight[0] = forward_weight[0] + size_pop;

		for (i = 1; i < num_obs; i++)
		{
			forward_attitude_vec[i] = forward_attitude_vec[i - 1] + 2 * size_pop;
			reverse_attitude_vec[i] = reverse_attitude_vec[i - 1] + 2 * size_pop;
			forward_weight[i] = forward_weight[i - 1] + 2 * size_pop;
			reverse_weight[i] = reverse_weight[i - 1] + 2 * size_pop;
		}

		ran_sample_n(&forward_noise_vec[0]->element[0], &gyrobox.gyro[0].param.sample.rate_noise,
					 2 * obs_size, sizeof(ebex_3vec_t) / sizeof(double));
		ran_sample_n(&forward_noise_vec[0]->element[1], &gyrobox.gyro[1].param.sample.rate_noise,
					 2 * obs_size, sizeof(ebex_3vec_t) / sizeof(double));
		ran_sample_n(&forward_noise_vec[0]->element[2], &gyrobox.gyro[2].param.sample.rate_noise,
					 2 * obs_size, sizeof(ebex_3vec_t) / sizeof(double));

		/// Generate reference error for no additional noise.
		ebex_quat_copy(&attitude, &start_attitude);
		restricted_num_obs = ebex_gyro_sc_quat_propagate_forward(model, gyrodata, &attitude, &gyrobox, NULL, 0, NULL, NULL, 0);

		ebex_quat_error(&(model->err_quats[m_pop->generation - 1]), &end_attitude, &attitude);
		model->err_weights[m_pop->generation - 1] = sqrt((double)restricted_num_obs);

		for (size_t pop_elem = 0; pop_elem < size_pop; pop_elem++)
		{
			/// Forward Segment
			ebex_quat_copy(&attitude, &start_attitude);
			restricted_num_obs = ebex_gyro_sc_quat_propagate_forward(
					model, gyrodata, &attitude, &gyrobox, &forward_attitude_vec[0][pop_elem], 2 * size_pop, NULL,
					forward_noise_vec[pop_elem], 0);

			forward_weight[0][pop_elem] = ebex_gyro_sc_get_probability(&end_attitude, &attitude, &gyrobox, restricted_num_obs,
					model->parameters->gyro_rate, segment_sc_variance);

			kahan_diff = forward_weight[0][pop_elem] - carry_weights[0];
			kahan_temp = total_weights[0] + kahan_diff;
			carry_weights[0] = (kahan_temp - total_weights[0]) - kahan_diff;
			total_weights[0] = kahan_temp;

			/// Reverse Segment
			ebex_quat_copy(&attitude, &end_attitude);
			ebex_gyro_sc_quat_propagate_backward(
					model, gyrodata, &attitude, &gyrobox, &reverse_attitude_vec[0][pop_elem], 2 * size_pop, NULL,
					reverse_noise_vec[pop_elem], 0, restricted_num_obs);

			reverse_weight[0][pop_elem] = ebex_gyro_sc_get_probability(&start_attitude, &attitude, &gyrobox, restricted_num_obs,
					model->parameters->gyro_rate, segment_sc_variance);
			kahan_diff = reverse_weight[0][pop_elem] - carry_weights[1];
			kahan_temp = total_weights[1] + kahan_diff;
			carry_weights[1] = (kahan_temp - total_weights[1]) - kahan_diff;
			total_weights[1] = kahan_temp;
		}

		ebex_steady_state_assign_weights(forward_weight, reverse_weight, total_weights, size_pop, restricted_num_obs);
//		ebex_steady_state_filter_attitude(model, forward_attitude_vec, reverse_attitude_vec, restricted_num_obs);

		///@TODO: This doesn't work when we are using multiple gyro boxes
		ebex_gyro_sc_output_time(model->parameters->steady_state.output, gyrodata->time, restricted_num_obs);
		ebex_gyro_sc_output_params(model->parameters->steady_state.output, &gyrobox, &sc_param, NULL, restricted_num_obs);

		///@TODO: This doesn't work when we are using multiple gyro boxes
#		pragma omp parallel for ordered \
			shared(forward_weight, reverse_weight, forward_attitude_vec, reverse_attitude_vec) private(attitude, cov) schedule(static)
		for (size_t obs = 0; obs < restricted_num_obs; obs++)
		{
			size_t best_dir = 0;
			size_t best_weight_elem = 0;
			double best_weight = 0.0;

			switch(model->parameters->steady_state.quaternion_filter_type)
			{
				case ebex_quat_filter_type_map:
					for (size_t pop_elem = 0; pop_elem < size_pop; pop_elem++)
					{
						if (best_weight < forward_weight[obs][pop_elem])
						{
							best_dir = 0;
							best_weight_elem = pop_elem;
							best_weight = forward_weight[obs][pop_elem];
						}
						if (best_weight < reverse_weight[obs][pop_elem])
						{
							best_dir = 1;
							best_weight_elem = pop_elem;
							best_weight = reverse_weight[obs][pop_elem];
						}
					}
					if (best_dir)
					{
						ebex_quat_copy(&attitude, &reverse_attitude_vec[obs][best_weight_elem]);
					}
					else
					{
						 ebex_quat_copy(&attitude, &forward_attitude_vec[obs][best_weight_elem]);
					}
					break;
				case ebex_quat_filter_type_weighted_mean:
					ebex_quat_spherical_weighted_mean(&attitude, forward_attitude_vec[obs], forward_weight[obs], 1.0 - ebex_slerp_tol, 100, 2 * size_pop);
					break;
				case ebex_quat_filter_type_mmse:
					ebex_quat_mmse(&attitude, forward_attitude_vec[obs], forward_weight[obs], 2 * size_pop);
					break;
			}
#			pragma omp ordered
			{
				ebex_gyro_sc_output_attitude(model->parameters->steady_state.output, &attitude, NULL, &sc_inv, 1);

				ebex_quat_generate_attitude_cov(&cov, forward_attitude_vec[obs], &attitude, forward_weight[obs], 2 * size_pop);
				ebex_gyro_sc_output_uncertainties(model->parameters->steady_state.output, &cov, 1);
				if(model->parameters->output_parent)
				{
					ebex_gyro_sc_output_parent(model->parameters->steady_state.output, &gyrodata->parent[obs], 1);
				}
			}
			model->parameters->steady_state.output->buffer_produced++;
		}

		switch(model->parameters->steady_state.quaternion_filter_type)
		{
			case ebex_quat_filter_type_weighted_mean:
				ebex_quat_spherical_weighted_mean(&attitude, forward_attitude_vec[restricted_num_obs - 1], forward_weight[0], 1.0 - ebex_slerp_tol, 100, size_pop);
				break;
			case ebex_quat_filter_type_mmse:
				ebex_quat_mmse(&attitude, forward_attitude_vec[restricted_num_obs - 1], forward_weight[0], size_pop);
				break;
			case ebex_quat_filter_type_map:
			{
				double best_weight;
				size_t best_weight_elem;
				for (size_t pop_elem = 0; pop_elem < size_pop; pop_elem++)
				{
					if (best_weight < forward_weight[restricted_num_obs - 1][pop_elem])
					{
						best_weight_elem = pop_elem;
						best_weight = forward_weight[restricted_num_obs - 1][pop_elem];
					}
				}
				ebex_quat_copy(&attitude, &(forward_attitude_vec[restricted_num_obs - 1][best_weight_elem]));
				break;
			}
		}
		ebex_quat_error(&err_quat, &attitude, &end_attitude);
		elog(log_level_verbose, "Filtered Quaternion error: %g\t Unfiltered Quaternion error:%g", 2.0 * acos(err_quat.w), 2.0 * acos(model->err_quats[m_pop->generation - 1].w));
		ebex_quat_copy(&model->err_quats[m_pop->generation - 1], &err_quat);

		e_free(forward_noise_vec[0]);
		e_free(forward_attitude_vec[0]);
		e_free(forward_weight[0]);

		e_free(forward_noise_vec);
		e_free(reverse_noise_vec);
		e_free(forward_attitude_vec);
		e_free(reverse_attitude_vec);
		e_free(forward_weight);
		e_free(reverse_weight);
	}
	ebex_data_write_buffer(model->parameters->steady_state.output);
}

static bool ebex_steady_state_pre_gen_hook(const size_t m_count, population_t *m_pop, organism_t *m_entity __attribute__((unused)))
{

	ebex_model_t *model = m_pop->data;

	if (shutdown_now)
	{
		elog(log_level_normal, "Received shutdown signal.  Exiting steady-state filter.");
		return false;
	}

	if (model->data->produce(model->data, 1) < 1)
	{
		elog(log_level_normal, "Could not read data for segment %zu", m_count);
		return false;
	}
	elog(log_level_extreme, "Population size is %zu at start of segment %zu", m_pop->size, m_count);

	return true;
}


static void ebex_steady_state_regenerate_population(population_t *m_pop)
{
	size_t new_size;
	organism_t *child;
	double weight;
	int *permutation;
	int i;

	/// Prepare arrays to store permutations.
	permutation = alloca(sizeof(int)*m_pop->size);
	for (i = 0; i < m_pop->size; i++)
		permutation[i] = i;

	/// Dither the weighting vector if requested (min_weight < max_weight
	weight = ran_double_range_uniform(m_pop->de_params->min_weight, m_pop->de_params->max_weight);

	m_pop->orig_size = m_pop->size;

	elog(log_level_normal, "Regenerating population!");
	for (i = 0; i < m_pop->orig_size; i++)
	{
		new_size = (size_t)round(m_pop->orig_size * m_pop->sorted_organisms[i]->previous_weight);
		while (new_size--)
		{
			child = genetic_organism_clone(m_pop, m_pop->sorted_organisms[i]);
			ebex_diffevolv_evolve_organism(m_pop, child, permutation, weight, 0, i);

			if (!m_pop->verify(m_pop, child))
			{
				genetic_free_organism(m_pop, child);
				new_size++;
			}
			child->previous_weight = 1.0 / (double)m_pop->orig_size;
		}
	}
	while (m_pop->orig_size--)
	{
		genetic_free_rank(m_pop, m_pop->orig_size);
	}

}

/**
 * Normalizes the current fitness weights and moves them to the previous weights
 * @param m_pop
 * @return Effective sample size (1/sum(weight^2))
 */
static double ebex_steady_state_step_normalize_weights(population_t *m_pop)
{
	double total_weight = 0.0;
	double total_squared_weight = 0.0;

	for (size_t i = 0; i < m_pop->size; i++)
	{
		total_weight += m_pop->sorted_organisms[i]->fitness;
	}
	for (size_t i = 0; i < m_pop->size; i++)
	{
		m_pop->sorted_organisms[i]->fitness /= total_weight;
		m_pop->sorted_organisms[i]->previous_weight = m_pop->sorted_organisms[i]->fitness;
		total_squared_weight += pow(m_pop->sorted_organisms[i]->previous_weight, 2.0);
	}

	return 1.0 / total_squared_weight;
}

static bool ebex_steady_state_compute_convergence_criteria(population_t *m_pop, const size_t m_count)
{
	ebex_model_t *model = m_pop->data;
	double total_weight = 0.0;
	double *weights;
	double *deltas;
	ebex_quat_t mean_error_quat;
	double sigma;
	double mean_error;
	size_t start_index = 0;

	weights = e_calloc(2 * m_count, sizeof(double));
	deltas = weights + m_count;

	if (m_count > model->parameters->steady_state.convergence_window_size)
	{
		start_index = m_count - model->parameters->steady_state.convergence_window_size;
	}

	for (size_t i = start_index; i < m_count; i++)
	{
		total_weight += model->err_weights[i];
	}
	for (size_t i = start_index; i < m_count; i++)
	{
		weights[i] = model->err_weights[i] / total_weight;
	}

	ebex_quat_spherical_weighted_mean(&mean_error_quat, model->err_quats + start_index, weights + start_index, 1.0 - ebex_slerp_tol, 100, m_count - start_index);
	mean_error = 3600.0 * (180.0 / M_PI) * 2.0 * acos(mean_error_quat.w);

	for (size_t i = start_index; i < m_count; i++)
	{
		deltas[i] = 2.0 * acos(model->err_quats[i].w);
	}

	sigma = 3600.0 * (180.0 / M_PI) *
			sqrt(ebex_stats_dbl_weighted_delta_variance(m_count - start_index, deltas + start_index, weights + start_index, false) / (double) (m_count - start_index));

	e_free(weights);

	elog(log_level_normal, "Step %zu error is %0.2f, over %zu steps the mean integrated error is %0.2f\" +/- %0.2f\" (at %0.2f sigma)",
	     m_count, acos(model->err_quats[m_count - 1].w) * 2.0 * 3600.0 * (180.0 / M_PI),
	     model->parameters->steady_state.convergence_window_size, mean_error,
	     sigma * model->parameters->steady_state.convergence_sigma, model->parameters->steady_state.convergence_sigma);

	return (mean_error > (model->parameters->steady_state.convergence_sigma * sigma)
			|| (m_count < model->parameters->steady_state.convergence_window_size)
			|| model->parameters->steady_state.convergence_continue
			);

}

static bool ebex_steady_state_post_gen_hook(const size_t m_count, population_t *m_pop, organism_t *m_entity __attribute__((unused)))
{
	ebex_model_t *model = m_pop->data;
	double effective_size;

	effective_size = ebex_steady_state_step_normalize_weights(m_pop);
	sort_population(m_pop);

	ebex_steady_state_output(m_pop);

	elog(log_level_verbose, "Effective population size is %f", effective_size);
	if ( effective_size < 0.75 * m_pop->stable_size)
		ebex_steady_state_regenerate_population(m_pop);


	model->data->consume(model->data, 1);
	return ebex_steady_state_compute_convergence_criteria(m_pop, m_count);
}

static void ebex_steady_state_cycle(population_t *m_pop, size_t m_numsegments)
{
	size_t segment = 0;
	ebex_model_t *model = (ebex_model_t *) m_pop->data;

	if (!m_pop)
		die("NULL pointer to population structure passed.");
	if (!m_pop->evaluate)
		die("Population's evaluation callback is undefined.");
	if (!m_pop->verify)
		die("Population's verification callback is undefined.");
	if (!m_pop->chromosome_constructor)
		die("Population's chromosome constructor callback is undefined.");
	if (!m_pop->chromosome_destructor)
		die("Population's chromosome destructor callback is undefined.");
	if (m_pop->stable_size < 10)
		die("Population's stable size is too small.  (Must be at least 10)");


	elog(log_level_verbose, "The steady state filter has begun!");

	m_pop->generation = 0;

	/// Create the initial population
	if (m_pop->size < m_pop->stable_size)
		genetic_fill_pop(m_pop, m_pop->stable_size - m_pop->size);

	while (segment < m_numsegments)
	{
		m_pop->generation = ++segment;

		if (!ebex_steady_state_pre_gen_hook(segment, m_pop, NULL)) break;
		ebex_genetic_eval_pop(m_pop);

		sort_population(m_pop);

		for (size_t generation = 0; generation < model->parameters->steady_state.generations_per_step; generation++)
		{
			elog(log_level_verbose, "Generation %zu", generation);

			m_pop->orig_size = m_pop->size;
			ebex_genetic_mating(m_pop);
			ebex_genetic_mutation(m_pop);
			ebex_genetic_adapt_eval(m_pop);
			ebex_genetic_elitism(m_pop);

		}
		if (!ebex_steady_state_post_gen_hook(segment, m_pop, m_pop->sorted_organisms[0])) break;
	}

}

void ebex_steady_state(ebex_model_t *m_model)
{
	population_t *ss_pop;

	ss_pop = genetic_pop_new(filter_config.steady_state.parameter_population_size, filter_config.num_gyros + filter_config.num_sc + num_gyro_params );

	ebex_diffevolv_setparameters(ss_pop,
			m_model->parameters->bootstrap.de_strategy,
			m_model->parameters->bootstrap.de_crossover,
			m_model->parameters->bootstrap.de_numvec,
			m_model->parameters->bootstrap.de_minweight,
			m_model->parameters->bootstrap.de_maxweight,
			m_model->parameters->bootstrap.de_crossover_ratio,
			m_model->parameters->bootstrap.de_opposition_rate,
			m_model->parameters->bootstrap.de_reference,
			m_model->parameters->bootstrap.de_scatter);

	ss_pop->seed = ebex_steady_state_organism_seed;
	ss_pop->chromosome_constructor = ebex_gyro_sc_generate_chromosome;
	ss_pop->chromosome_destructor = ebex_genetic_chromosome_free;
	ss_pop->verify = ebex_gyro_sc_organism_verify;
	ss_pop->evaluate = ebex_steady_state_organism_evaluate;

	ss_pop->crossover_ratio = m_model->parameters->steady_state.mating_ratio;
	ss_pop->mutation_ratio = m_model->parameters->steady_state.organism_mutation_likelihood;
	ss_pop->mutation_type = (e_genetic_mutation_type) m_model->parameters->steady_state.mutation_type;
	ss_pop->select_one_type = (e_genetic_select_type) m_model->parameters->steady_state.mutation_selection_type;
	ss_pop->select_two_type = (e_genetic_select_type) m_model->parameters->steady_state.mating_selection_type;

	ss_pop->scheme = (evolution_mode_t) m_model->parameters->steady_state.evolution_type;
	ss_pop->elitism = m_model->parameters->steady_state.elitism_type;

//	ss_pop->pre_generation_hook = ebex_steady_state_pre_gen_hook;
//	ss_pop->post_generation_hook = ebex_steady_state_post_gen_hook;

	if (m_model->parameters->steady_state.eval_segments == 0)
		m_model->parameters->steady_state.eval_segments = 1;
	if (m_model->parameters->steady_state.minimum_effective_size == 0)
		m_model->parameters->steady_state.minimum_effective_size = (size_t) (0.75 * (double)m_model->parameters->steady_state.parameter_population_size);
	if (m_model->parameters->steady_state.convergence_sigma <= 0.0)
		m_model->parameters->steady_state.convergence_sigma = 1.0;
	if (m_model->parameters->steady_state.convergence_window_size == 0)
		m_model->parameters->steady_state.convergence_window_size = 50;
	if (m_model->parameters->steady_state.learning_ratio <= 0.0)
		m_model->parameters->steady_state.learning_ratio = 0.1;

	m_model->private_data = ss_pop;
	ss_pop->data = m_model;

	ebex_gyro_sc_register_parent_output(m_model, &m_model->parameters->steady_state.output, m_model->parameters->steady_state.output_filename, m_model->parameters->steady_state.output_type);
	ebex_gyro_sc_register_attitude_output(m_model, &m_model->parameters->steady_state.output, m_model->parameters->steady_state.output_filename, m_model->parameters->steady_state.output_type);
	ebex_gyro_sc_register_param_output(&m_model->parameters->steady_state.output, m_model->parameters->steady_state.output_filename, m_model->parameters->steady_state.output_type);
	ebex_gyro_sc_register_uncertainty_output(&m_model->parameters->steady_state.output, m_model->parameters->steady_state.output_filename, m_model->parameters->steady_state.output_type);

	m_model->data->produce(m_model->data, m_model->parameters->steady_state.eval_segments);
	ebex_steady_state_cycle(ss_pop, m_model->parameters->steady_state.max_segments);

	ebex_data_close_source(m_model->parameters->steady_state.output);
	ss_pop->data = NULL;

	genetic_close(ss_pop);
}
